#include "Vehicle.h"
#include "Time.h"
#include "ECS/Algorithms/AStar.h"
#include "../Pathfinding/Landscape8DirNode.h"
#include "../Landscape.h"
#include <cmath>

mtBEGIN(VehicleDef)
	mtBASE(ActuatorDef);
	mtCONSTRUCTOR();
mtEND

mtBEGIN(Vehicle)
	mtBASE(Actuator);
	mtCONSTRUCTOR();
	mtMETHOD(initWith);
	mtMETHOD(onStart);
	mtMETHOD(onUpdate);
mtEND


bool Vehicle::initWith( VehicleDef* def )
{
	if (!Actuator::initWith(def))
		return false;

	_speed = 1.0f;
	return true;
}

void Vehicle::onUpdate()
{
	if (_target.hasValue())
	{
		float length = _speed * Time::deltaTime;
		if (_route.empty())
		{
			doMoveTo(_target.value(), length);
		}
		else
		{
			while (length > 0.0f && !_route.empty())
			{
				if (doMoveTo(_route[0], length))
				_route.erase(_route.begin());
			}
		}
	}
}

void Vehicle::onStart()
{
	_map = Landscape::getInstance()->getMap();
	_cachedTransform = getComponent<Transform>();
}

bool Vehicle::hasTarget() const
{
	return _target.hasValue();
}

void Vehicle::clearTarget()
{
	_target = nullptr;
	_route.clear();
}

void Vehicle::setTarget(const ECS::Vec2<float>& point)
{
	_target = point;
	if (!findPath(ECS::Vec2<int>(_cachedTransform->getPosition()), ECS::Vec2<int>(_target.value()), &_route))
	{
		_target = nullptr;
	}
}

bool Vehicle::findPath(  const ECS::Vec2<int>& start, const ECS::Vec2<int>& end, Route* path )
{
	ECS::AStarSearch<Landscape8DirNode> astarsearch;

	Landscape8DirNode nodeStart(_map, start.x, start.y);

	// Define the goal state
	Landscape8DirNode nodeEnd(_map, end.x, end.y);

	// Set Start and goal states
	astarsearch.SetStartAndGoalStates( nodeStart, nodeEnd );

	unsigned int SearchState;

	unsigned int SearchSteps = 0;
	path->clear();
	do
	{
		SearchState = astarsearch.SearchStep();

		SearchSteps++;
	}
	while( SearchState == ECS::AStarSearch<Landscape8DirNode>::SEARCH_STATE_SEARCHING );

	if( SearchState == ECS::AStarSearch<Landscape8DirNode>::SEARCH_STATE_SUCCEEDED )
	{

		Landscape8DirNode *node = astarsearch.GetSolutionStart();
		int steps = 0;

		for( ;; )
		{
			node = astarsearch.GetSolutionNext();


			if( !node )
			{
				break;
			}

			path->push_back(ECS::Vec2<int>(node->getX(), node->getY()));

			steps ++;

		};

		node = astarsearch.GetSolutionEnd();
	}
	return !path->empty();
}

bool Vehicle::doMoveTo( const ECS::Vec2<float>& wayPoint, float& length )
{
	ECS::Vec2<float> direction = (wayPoint - _cachedTransform->getPosition()).normalized();
	float distance = (wayPoint - _cachedTransform->getPosition()).magnitude();
	ECS::Vec2<float> newPos;

	if (direction.magnitudeSq() > 0.0f)
	{
		_cachedTransform->setAngle(std::atan2(direction.y, direction.x));
	}

	if (distance <= length)
	{
		newPos = wayPoint;
		_cachedTransform->setPosition(newPos);
		length -= distance;
		return true;
	}
	else
	{
		newPos = _cachedTransform->getPosition() + direction * length;
		_cachedTransform->setPosition(newPos);
		length = 0.0f;
		return false;
	}
}
